
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       p_2d.c
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "p_2d.h"
#include "p.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Constructor
void p_2d_ctrl_ctor(P_2d_ctrl* controler, float initial_p, float dt)
{
    controler->_dt = dt;
    controler->_kp = initial_p;
}

// update_all - set target and measured inputs to P controller and calculate outputs
// limit is set true if the target has been moved to limit the maximum position error
Vector2f_t p_2d_ctrl_update_all(P_2d_ctrl* controler, postype_t *target_x, postype_t *target_y, const Vector2f_t *measurement, bool *limit)
{
    *limit = false;

    // calculate distance _error
    controler->_error.x = *target_x - measurement->x;
    controler->_error.y = *target_y - measurement->y;

    // Constrain _error and target position
    // Constrain the maximum length of _vel_target to the maximum position correction velocity
    if (math_flt_positive(controler->_error_max) && vec2_limit_length(&controler->_error, controler->_error_max)) {
        *target_x = measurement->x + controler->_error.x;
        *target_y = measurement->y + controler->_error.y;
        *limit = true;
    }

    // MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded
    return p_2d_ctrl_sqrt_controller(&controler->_error, controler->_kp, controler->_D1_max, controler->_dt);
}

// set_limits - sets the maximum error to limit output and first and second derivative of output
// when using for a position controller, lim_err will be position error, lim_out will be correction velocity, lim_D will be acceleration, lim_D2 will be jerk
void p_2d_ctrl_set_limits(P_2d_ctrl* controler, float output_max, float D_Out_max, float D2_Out_max)
{
    controler->_D1_max = 0.0f;
    controler->_error_max = 0.0f;

    if (math_flt_positive(D_Out_max)) {
        controler->_D1_max = D_Out_max;
    }

    if (math_flt_positive(D2_Out_max) && math_flt_positive(controler->_kp)) {
        // limit the first derivative so as not to exceed the second derivative
        controler->_D1_max = MIN(controler->_D1_max, D2_Out_max / controler->_kp);
    }

    if (math_flt_positive(output_max) && math_flt_positive(controler->_kp)) {
        controler->_error_max = inv_sqrt_controller(output_max, controler->_kp, controler->_D1_max);
    }
}

// set_error_max - reduce maximum position error to error_max
// to be called after setting limits
void p_2d_ctrl_set_error_max(P_2d_ctrl* controler, float error_max)
{
    if (math_flt_positive(error_max)) {
        if (!math_flt_zero(controler->_error_max) ) {
            controler->_error_max = MIN(controler->_error_max, error_max);
        } else {
            controler->_error_max = error_max;
        }
    }
}


// proportional controller with piecewise sqrt sections to constrain second derivative
Vector2f_t p_2d_ctrl_sqrt_controller(const Vector2f_t* error, float p, float second_ord_lim, float dt)
{
    Vector2f_t output = {0};
    const float error_length = vec2_length(error);
    if (!math_flt_positive(error_length)) {
        return output;
    }

    const float correction_length = p_ctrl_sqrt_controller(error_length, p, second_ord_lim, dt);
    vec2_mult(&output, error, correction_length / error_length);

    return output;
}

/*------------------------------------test------------------------------------*/


